#include "apf.h"

using namespace NS_APF;

Vec2 node::attractive_force_calculate(Vec2 _object_position, float _k_a, float _goal_max_distance)
{
    float distance = sqrt(pow(_object_position.x - this->position_.x, 2) +
        pow(_object_position.y - this->position_.y, 2));

    if(distance > _goal_max_distance)
        distance = _goal_max_distance;
    else if(distance < 5){
        return Vec2(0,0);
    }
    float theta = atan2(_object_position.y - this->position_.y,
                        _object_position.x - this->position_.x);

    float combined_force = _k_a * distance;

    Vec2 attractive_force(combined_force * cos(theta),
                           combined_force * sin(theta));
    std::cout << "the attractive_force between UAV is: " << attractive_force << std::endl;

//    std::cout << "current theta of UAV node is " << theta * 180 /3.14159<< std::endl;
//    std::cout << "current attractive forve of UAV node is " << attractive_force_ << std::endl;
//    std::cout << "current position of UAV node is " << this->position_ << ", goal is " << _object_position << std::endl;
    return attractive_force;
}

Vec2 node::spring_force_calculate(Vec2 _object_position, Vec2 relative_position, float _k_s)
{
    float distance =sqrt(pow(_object_position.x - this->position_.x - relative_position.x, 2) +
                         pow(_object_position.y - this->position_.y - relative_position.y, 2));

    float theta = atan2(_object_position.y - this->position_.y - relative_position.y,
                        _object_position.x - this->position_.x - relative_position.x);

    float combined_force = _k_s * distance;

    Vec2 spring_force(combined_force * cos(theta),
                      combined_force * sin(theta));
    std::cout << "the spring force between UAV is: " << spring_force << std::endl;

    return spring_force;
}

Vec2 node::revolution_force_calculate(Vec2 _object_position, Vec2 goal, float _k_r, float _dilation, float _detect_radius)
{
    float distance = sqrt(pow(_object_position.x - this->position_.x, 2) +
        pow(_object_position.y - this->position_.y, 2));

    if(distance > _detect_radius) {
        return Vec2(0,0);
    } else {
        float dis_att = sqrt(pow(goal.x - this->position_.x, 2) + pow(goal.y - this->position_.y, 2));

//      Vec2 result (tmp * (this->position_.x - _object_position.x) + ((rand() % 2000) - 200) / 1.f,
//                      tmp * (this->position_.y - _object_position.y) + ((rand() % 2000) - 200) / 1.f) ;

        float tmp_1 = _k_r * (1 / distance - 1 / _detect_radius) / pow(distance, 2) * pow(dis_att, 2);
        Vec2 revolution_force_1(tmp_1 * (this->position_.x - _object_position.x) /distance,
                    tmp_1 * (this->position_.y - _object_position.y) / distance);

        float tmp_2 = _k_r * pow( 1 / distance - 1 / _detect_radius, 2) * dis_att;
        Vec2 revolution_force_2(tmp_2 * (goal.x - this->position_.x)/ dis_att,
                                tmp_2 * (goal.x - this->position_.y)/ dis_att );

        Vec2 result =  revolution_force_1 + revolution_force_2;
        std::cout << "the revolution_force_1 UAV is: " << revolution_force_1 << std::endl;
        std::cout << "the revolution_force_2 UAV is: " << revolution_force_2 << std::endl;
        std::cout << "the revolution force between UAV is: " << result << std::endl;

        return result;
    }
}

APF::APF(APF_Param* _param_p, Vec2 _goal, int _fol_num)
{
    fol_num_    = _fol_num;
    param_.k_a_ = _param_p->k_a_;
    param_.k_r_ = _param_p->k_r_;
    param_.k_s_ = _param_p->k_s_;
    param_.goal_max_distance_ = _param_p->goal_max_distance_;
    param_.detect_radius_ = _param_p->detect_radius_;
    param_.UAV_virtual_mass = _param_p->UAV_virtual_mass;
    param_.init_UAV_pos_ = _param_p->init_UAV_pos_;
    param_.obstacle_position_ = _param_p->obstacle_position_;
    param_.UAV_max_speed_ = _param_p->UAV_max_speed_;
    param_.obstacle_dilation_ = _param_p->obstacle_dilation_;
    goal_ = _goal;

    std::vector<node> fol_node(fol_num_);

    UAV_List_.insert(UAV_List_.end(), fol_node.begin(), fol_node.end());

    std::vector<std::vector<int>> adjacent_tmp = {
        {0, 1, 1, 0, 0},
        {1, 0, 1, 1, 0},
        {1, 1, 0, 0, 1},
        {0, 1, 0, 0, 1},
        {0, 0, 1, 1, 0},

    };
    adjacent_ = adjacent_tmp;

    for(int i = 0; i < adjacent_.size(); i++) {
        for(int j = 0; j < adjacent_[i].size(); j++){
            std::cout<< "i = "<<i<< " "<< "j = "<<j<< "  " <<adjacent_.at(i).at(j)<< std::endl;
        }
    }
    for(int i = 0; i < _fol_num+1; i++) {
        UAV_List_[i].position_ = param_.init_UAV_pos_[i-1];
    }
}



void APF::UAV_combine_foce_calculate(int _UAV_index)
{
    UAV_List_[_UAV_index].revolution_force_.clear();
    for(auto obstack : param_.obstacle_position_) {
        UAV_List_[_UAV_index].revolution_force_ = UAV_List_[_UAV_index].revolution_force_ +
            UAV_List_[_UAV_index].revolution_force_calculate(obstack,
                                                            goal_,
                                                            param_.k_r_,
                                                            param_.obstacle_dilation_,
                                                            param_.detect_radius_);
    }

    UAV_List_[_UAV_index].spring_force_.clear();
    for(int index = 0; index < fol_num_+1; index++){
        if (adjacent_[_UAV_index][index] == 0)
            continue;

        UAV_List_[_UAV_index].spring_force_ = UAV_List_[_UAV_index].spring_force_ +
            UAV_List_[_UAV_index].spring_force_calculate(UAV_List_[index].position_,
                                                         param_.init_UAV_pos_[index] - param_.init_UAV_pos_[_UAV_index],
                                                         param_.k_s_);
    }

    UAV_List_[_UAV_index].attractive_force_.clear();
    UAV_List_[_UAV_index].attractive_force_ =
            UAV_List_[_UAV_index].attractive_force_calculate(goal_, param_.k_a_, param_.goal_max_distance_);

    UAV_List_[_UAV_index].sum_force_ = UAV_List_[_UAV_index].attractive_force_
                                        + UAV_List_[_UAV_index].revolution_force_
                                        + UAV_List_[_UAV_index].spring_force_;
}

Vec2 APF::UAV_velocity_calculate(int _UAV_index, Vec2 _current_position)
{
    UAV_List_[_UAV_index].position_update(_current_position);

    UAV_combine_foce_calculate(_UAV_index);
    UAV_List_[_UAV_index].velocity_ = /*UAV_List_[_UAV_index].velocity_ +*/
        UAV_List_[_UAV_index].sum_force_ / param_.UAV_virtual_mass;

    float absolute_velocity = UAV_List_[_UAV_index].velocity_.mod();
    float velocity_orient =atan2(UAV_List_[_UAV_index].velocity_.y , UAV_List_[_UAV_index].velocity_.x);

    if(absolute_velocity > param_.UAV_max_speed_)
        UAV_List_[_UAV_index].velocity_ = Vec2(param_.UAV_max_speed_ * cos(velocity_orient), param_.UAV_max_speed_ * sin(velocity_orient));

    return UAV_List_[_UAV_index].velocity_;
}



std::vector<Vec2> APF::calculate(std::vector<Vec2> _current_position_list)
{
    std::vector<Vec2> result(fol_num_+1);

    for(int index=0; index < fol_num_+1; index++){
        result[index] = UAV_velocity_calculate(index, _current_position_list[index]);
    }
    return result;
}

double APF::goal_distance()
{
   return sqrt(pow((UAV_List_[0].position_.x - goal_.x), 2) +
               pow((UAV_List_[0].position_.y - goal_.y), 2) );
}

void APF::add_obstacle(Vec2 _start, Vec2 _end)
{
    //生成直线障碍物
    Vec2 direction = _end - _start;
    float distance = sqrt(direction.x * direction.x + direction.y * direction.y);
    float step = 1.0f; // 可以根据需要调整步长
    direction.x /= distance;
    direction.y /= distance;
    for (float i = 0; i <= distance; i += step) {
        Vec2 point = Vec2(_start.x + direction.x * i, _start.y + direction.y * i);
        //param_.obstacle_position_.push_back(point);
    }

    // 随机生成障碍物
    srand((unsigned)time(NULL));
    for(int i = 0; i < 8; i++){

        float x = rand() / static_cast<double>(RAND_MAX)+ rand() % 800;
        float y = rand() / static_cast<double>(RAND_MAX)+ rand() % 600;

        Vec2 point = Vec2(x,y);
        param_.obstacle_position_.push_back(point);
    }
}










